/*
 * This file is part of the Chumby Computer Vision Project: www.EMGRobotics.com
 *
 * Copyright (c) 2010 individual Chumby Computer Vision contributors. 
 * See end of header for contributor list.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 *
 * In all cases you must keep this copyright notice intact, along with the
 * list of contributors at the end of this header.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 *
 * Contributors: 
 *			Eric Gregori ( www.EMGRobotics.com )
 */
//
//**********************************************************************************
//
//  ChumbyCV - Chumby Computer Vision Application framework
//
//  This framework provides a interface to a V4L2 camera(input), and a interface to a
//  Linux framebuffer(output).
//  This very simple framework makes it easy to develop Computer Vision applications
//  for the Chumby internet device.
//  This Framework has been tested on Chumby Ones, using various Logitech USB cameras.
//  See www.EMGRobotics.com for more information.
//
//  Video 4 Linux version 2 interface ( V4L2 )
//  This code started as: http://v4l2spec.bytesex.org/spec/capture-example.html
//
//**********************************************************************************
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/mman.h>
#include <errno.h>
#include <sys/stat.h>
#include <assert.h>
#include <stdint.h>
#include <linux/fs.h>
#include <linux/kernel.h>
#include <asm/types.h>          /* for videodev2.h */
#include <linux/videodev2.h>

#define CLEAR(x) memset (&(x), 0, sizeof (x))

//**********************************************************************************
// Structures
//**********************************************************************************
struct buffer {
	void *	start;
        size_t  length;
};

//**********************************************************************************
// Global
//**********************************************************************************
static int              fd              = -1;
static struct buffer 	*buffers        = NULL;
static unsigned int     n_buffers       = 0;
static struct 		v4l2_buffer 	BufferDescriptor;

//************************************************************************************
//*************************************************************************************
//****************************                       **********************************
//****************************    Capture Start      **********************************
//****************************                       **********************************
//*************************************************************************************
//*************************************************************************************
static void errno_exit( const char *s )
{
	fprintf (stderr, "%s error %d, %s\n",s, errno, strerror (errno));
	exit (EXIT_FAILURE);
}

static int xioctl( int fd, int request, void *arg)
{
        int r;

        do r = ioctl (fd, request, arg);
        while (-1 == r && EINTR == errno);

        return r;
}


static void uninit_device(void)
{
	unsigned int i;

	for (i = 0; i < n_buffers; ++i)
	{
		if (-1 == munmap (buffers[i].start, buffers[i].length))
			errno_exit ("munmap");
	}
	free (buffers);
}



static void init_mmap(void)
{
	struct v4l2_requestbuffers req;

	printf( "\ninit_mmap" );
        CLEAR (req);
        req.count               = 4;
        req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        req.memory              = V4L2_MEMORY_MMAP;

	if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) 
	{
                if (EINVAL == errno) 
		{
                        printf ("does not support memory mapping\n");
                        exit (EXIT_FAILURE);
                } 
		else 
		{
                        errno_exit ("VIDIOC_REQBUFS");
                }
        }

        if (req.count < 2) 
	{
                printf ("Insufficient buffer memory\n");
                exit (EXIT_FAILURE);
        }

        buffers = calloc (req.count, sizeof (*buffers));

        if (!buffers) 
	{
                printf ("Out of memory\n");
                exit (EXIT_FAILURE);
        }

        for (n_buffers = 0; n_buffers < req.count; ++n_buffers) 
	{
                struct v4l2_buffer buf;

                CLEAR (buf);
                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory      = V4L2_MEMORY_MMAP;
                buf.index       = n_buffers;

                if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
                        errno_exit ("VIDIOC_QUERYBUF");

		printf( "\nbuffer length = %d", buf.length );
                buffers[n_buffers].length = buf.length;
                buffers[n_buffers].start =
                        mmap (NULL /* start anywhere */,
                              buf.length,
                              PROT_READ | PROT_WRITE /* required */,
                              MAP_SHARED /* recommended */,
                              fd, buf.m.offset);

                if (MAP_FAILED == buffers[n_buffers].start)
                        errno_exit ("mmap");
        }
}



static void init_device( int width, int height )
{
        struct v4l2_capability cap;
        struct v4l2_cropcap cropcap;
        struct v4l2_crop crop;
        struct v4l2_format fmt;
	unsigned int min;

        if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) 
	{
		if (EINVAL == errno) 
		{
			printf ("not a V4L2 device\n" );
                        exit (EXIT_FAILURE);
                } 
		else 
		{
                        errno_exit ("VIDIOC_QUERYCAP");
                }
        }

        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) 
	{
                printf ("not a video capture device\n" );
                exit (EXIT_FAILURE);
        }

	if (!(cap.capabilities & V4L2_CAP_STREAMING)) 
	{
		fprintf (stderr, "does not support streaming i/o\n" );
		exit (EXIT_FAILURE);
	}

	CLEAR (cropcap);
        cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) 
	{
		crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                crop.c = cropcap.defrect; /* reset to default */

                if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) 
		{
                	switch (errno) 
			{
                        	case EINVAL:
                                	/* Cropping not supported. */
                                	break;
                        	default:
                                	/* Errors ignored. */
                                	break;
                        }
                }
	}
 
        CLEAR (fmt);
        fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width       = width; 
        fmt.fmt.pix.height      = height;
        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
        fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

        if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
                errno_exit ("VIDIOC_S_FMT");

        /* Note VIDIOC_S_FMT may change width and height. */

	/* Buggy driver paranoia. */
	min = fmt.fmt.pix.width * 2;
	if (fmt.fmt.pix.bytesperline < min)
		fmt.fmt.pix.bytesperline = min;
	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
	if (fmt.fmt.pix.sizeimage < min)
		fmt.fmt.pix.sizeimage = min;

	fprintf( stderr, "Init Complete\n" );
	init_mmap ();
}

static void close_device(void)
{
        if (-1 == close (fd))
	        errno_exit ("close");

        fd = -1;
}

static void open_device( char* dev_name )
{
        struct stat st; 

        if (-1 == stat (dev_name, &st)) 
	{
                fprintf (stderr, "Cannot identify '%s': %d, %s\n", dev_name, errno, strerror (errno));
                exit (EXIT_FAILURE);
        }

        if (!S_ISCHR (st.st_mode)) 
	{
                fprintf (stderr, "%s is no device\n", dev_name);
                exit (EXIT_FAILURE);
        }

        fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);

        if (-1 == fd) 
	{
                fprintf (stderr, "Cannot open '%s': %d, %s\n", dev_name, errno, strerror (errno));
                exit (EXIT_FAILURE);
        }

	printf( "\n%s is Open - ", dev_name );
}

//----------------------------------------------------------------------------------

//***********************************************************************************
// Start Capturing from the V4L2 device
//***********************************************************************************
void start_capturing( char* dev_name, int width, int height )
{
        unsigned int i;
        enum v4l2_buf_type type;

	open_device( dev_name );
	init_device( width, height );

	for (i = 0; i < n_buffers; ++i) 
	{
        	struct v4l2_buffer buf;

        	CLEAR (buf);
        	buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        	buf.memory      = V4L2_MEMORY_MMAP;
        	buf.index       = i;

        	if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
                	errno_exit ("VIDIOC_QBUF");
	}
		
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
		errno_exit ("VIDIOC_STREAMON");

	fprintf( stderr, "\nstart_capturing" );
}

//***********************************************************************************
// Stop Captruing, and release resources
//***********************************************************************************
void stop_capturing(void)
{
	enum v4l2_buf_type type;

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
		errno_exit ("VIDIOC_STREAMOFF");

        uninit_device ();
        close_device ();
}

//***********************************************************************************
// Get a single frame from the camera in the YUYV format.
//
// YUYV - http://v4l2spec.bytesex.org/spec/r4339.htm
// y1 = data[CaptureIndex++];
// u  = data[CaptureIndex++];
// y2 = data[CaptureIndex++];
// v  = data[CaptureIndex++];
//
// Returns a pointer to the first byte in the framebuffer (y1).
//***********************************************************************************
unsigned char *GetFrame( void )
{
	CLEAR (BufferDescriptor);
        BufferDescriptor.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        BufferDescriptor.memory = V4L2_MEMORY_MMAP;

    	if (-1 == xioctl (fd, VIDIOC_DQBUF, &BufferDescriptor)) 
	{
        	switch (errno) 
                {
            		case EAGAIN:
                    		return(NULL);

			case EIO:
				/* Could ignore EIO, see spec. */
				/* fall through */
			default:
				errno_exit ("VIDIOC_DQBUF");
		}
	} // xioctl

        assert (BufferDescriptor.index < n_buffers);

	// image is pointed to by (buffers[tempbuf.index].start)
	// capture is setup for yuy2
	return((unsigned char *)buffers[BufferDescriptor.index].start);
}

//***********************************************************************************
// ReleaseFrame - release the framebuffer back to the V4L2 driver
//***********************************************************************************
void ReleaseFrame(void)
{
	if (-1 == xioctl (fd, VIDIOC_QBUF, &BufferDescriptor))
		errno_exit ("VIDIOC_QBUF");
}




